#include "st.h"


using namespace cv;

void Stat::invertMatrix(InputArray src, OutputArray st) {
    invert(src, st, DECOMP_SVD);
    //std::cout<<"["<<s<<"]"<<std::endl;
}

void Stat::covarMatrix(InputArray samples, OutputArray covar, OutputArray mean) {
    calcCovarMatrix(samples, covar, mean, CV_COVAR_SCALE | CV_COVAR_NORMAL | CV_COVAR_ROWS, CV_32FC1);
}

void Stat::stDev(std::vector< std::vector<float> > samples, float *stdev){

    int size= samples.size();
    float meanVector[15];
    for(int i = 0;i<15;i++){
        meanVector[i]=0;
        stdev[i]=0;
    }
    
    for(int i = 0 ; i< 15; i++){
        for(int j = 0 ; j < size ; j++){
            meanVector[i]=meanVector[i]+samples.at(j).at(i);
        }
    }
    //std::cout<<"mean"<<std::endl;
    for(int i = 0 ; i< 15; i++){
            meanVector[i]=meanVector[i]/size;
            //std::cout<<meanVector[i] << " ";
    }
    //std::cout<<std::endl;
    
    for(int i = 0 ; i< 15;i++){
        for(int j = 0; j< size; j++){
            stdev[i]=stdev[i]+(samples.at(j).at(i)-meanVector[i])*(samples.at(j).at(i)-meanVector[i]);
            //std::cout<< stdev[j]<<" ";
        }
    }
    
    //std::cout<<"stdev"<<std::endl;
    for(int i = 0 ; i < 15 ; i++){
        stdev[i]=sqrt(stdev[i]);
        stdev[i]=stdev[i]/size;
        //std::cout<< stdev[i]<<" ";
    }
    //std::cout<<std::endl;
    //std::cout<<"50"<<std::endl;

}

/*
 float* Stat::stDev(std::vector< std::vector<float> > samples) {
    //calcula matriz de covariancias
    using namespace cv;
    int numberOfSamples = samples.size();
    float arrsamples[numberOfSamples][15];
    for(int i = 0 ; i < numberOfSamples ; i++){
        for(int j = 0 ; j <  15 ; j++){
            arrsamples[i][j] = samples.at(i).at(j);
            std::cout << arrsamples[i][j] << std::endl;
        }
    }
    Mat samplesMat(numberOfSamples, 15, CV_32FC1, (void*) arrsamples);
    Mat covars(15, 15, CV_32FC1);
    Mat matmean(15, 1, CV_32FC1);
    covarMatrix(samplesMat, covars, matmean);

    //std::cout<< covars << std::endl;
    
    float stDev[15];
    for (int i = 0; i < 15; i++) {
        std::cout<<"covars.at"<<covars.at<float>(i,i)<<std::endl;
        stDev[i] = sqrt(covars.at<float>(i, i));
        std::cout<<"stdev"<<stDev[i]<<std::endl;
    }
    return stDev;

}
 */

float Stat::anglePlanes(std::vector<float> plane1, std::vector<float> plane2){
    float a0 = plane1.at(0);
    float a1 = plane1.at(1);
    float a2 = plane1.at(2);
    
    float b0 = plane2.at(0);
    float b1 = plane2.at(1);
    float b2 = plane2.at(2);
    
    return 180-(180/3.14159265359)*acos((a0*b0+a1*b1+a2*b2)/(sqrt(a0*a0+a1*b1+a2*a2)*sqrt(b0*b0+b1*b1+b2*b2)));
}

std::vector<float> Stat::fitPlane(std::vector< std::vector<float> > pointsV){
    using namespace cv;
    //A*B = C
    //matriz A
    for(int i = 0 ; i < pointsV.size();i++){
        pointsV.at(i).push_back(1);
        //std::cout<<pointsV.at(i).at(3)<<std::endl;
    }
    //passa matriz A para um array
    float ps[pointsV.size()][4];
    for(int i = 0 ; i < pointsV.size();i++){
        for(int j = 0 ; j < 4 ; j++){
            ps[i][j]=pointsV.at(i).at(j);
        }
    }
    //Mat do array da matriz A
    Mat pointsMat(pointsV.size(),4, CV_32FC1, (void*) ps);
    /*for(int i = 0 ; i < 5; i++){
        for(int j = 0 ; j < 4 ; j++){
            std::cout << pointsMat.at<float>(i,j)<<" ";
        }
        std::cout << std::endl;
    }*/
    //std::cout<<"points "<<pointsMat<<std::endl;
    //vetor que recebera o plano
    std::vector< float > plane(4);
    //Mat que representa a matriz B em A*B=C
    Mat planeMat(4,1,CV_32FC1);
    //coloca a solução do sistema linear em planeMat
    SVD::solveZ(pointsMat, planeMat);
    
    //std::cout<<"rows "<<planeMat.rows<<" columns "<< planeMat.cols<<std::endl;
    //std::cout<<planeMat<<std::endl;
    
    //coloca valores no vetor para ser retornado pela função
    for(int i = 0 ; i < 4 ; i ++){
        plane.at(i)=planeMat.at<float>(i,0);
    }
    
    return plane;
}


std::vector<float> Stat::fitPlanef(std::vector< std::vector<float> > pointsV){
    using namespace cv;
    
    float a[pointsV.size()][3];
    
    for(int i = 0 ; i < pointsV.size(); i++){
        for(int j = 0 ; j < 3  ; j++){
            a[i][j]=pointsV.at(i).at(j);
            if(j==2){
                a[i][2]=1;
            }
        }
    }
    
    float z[pointsV.size()][1];
    for(int i = 0 ; i < pointsV.size() ; i++){
        a[i][0]=pointsV.at(i).at(2);
    }
    Mat A(pointsV.size(),3,CV_32FC1,(void*) a);
    Mat X(3,1,CV_32FC1);
    Mat Z(pointsV.size(),1,CV_32FC1,(void*) z);
    
    std::vector<float> plane(4);
    plane.at(0)=X.at<float>(0,0);
    plane.at(1)=X.at<float>(1,0);
    plane.at(2)=1;
    plane.at(3)=X.at<float>(2,0);
    
    return plane;
}

float distance(float *a, float *b) {

    return
    sqrt((a[0] - b[0])*(a[0] - b[0])+(a[1] - b[1])*(a[1] - b[1])+(a[2] - b[2])*(a[2] - b[2]));
}

float skeletonDistance(pi_tracker::Skeleton::ConstPtr msg, int a, int b) {

    float u[3];
    float v[3];

    u[0] = (float) msg->position[a].x;
    u[1] = (float) msg->position[a].y;
    u[2] = (float) msg->position[a].z;

    v[0] = (float) msg->position[b].x;
    v[1] = (float) msg->position[b].y;
    v[2] = (float) msg->position[b].z;

    return distance(u, v);

}

float Stat::angleVectors(float a1,float a2, float a0, float b1, float b2, float b0){
  
  return 180-(180/3.14159265359)*acos((a0*b0+a1*b1+a2*b2)/(sqrt(a0*a0+a1*b1+a2*a2)*sqrt(b0*b0+b1*b1+b2*b2)));
  
}